package org.femto.math.linalg;

public class Matrix4x4 {

    public double[] m = new double[16];

    public Matrix4x4() {
        setIdentity();
    }

    public Matrix4x4(Matrix4x4 A) {
        set(A);
    }

    public Matrix4x4(Vec3 d) {
        setDiagonal(d);
    }

    public Matrix4x4(double v11, double v12, double v13, double v14,
            double v21, double v22, double v23, double v24, double v31,
            double v32, double v33, double v34, double v41, double v42,
            double v43, double v44) {
        set(v11, v12, v13, v14, v21, v22, v23, v24, v31, v32, v33, v34, v41, v42, v43, v44);
    }

    public Matrix4x4(double[] array) {
        for (int i = 0; i < 16; i++) {
            m[i] = array[i];
        }
    }

    public void m(int i, int j, double a) {
        m[j+i*4] = a;
    }

    public double m(int i, int j) {
        return m[j+i*4];
    }

    public Vec3 getBasis(int i) {
        return new Vec3(m[0+i*4], m[1+i*4], m[2+i*4]);
    }

    public String toString() {
        return "[" + m[0] + " " + m[4] + " " + m[8] + " " + m[12] + "; "
                + m[1] + " " + m[5] + " " + m[9] + " " + m[13] + "; "
                + m[2] + " " + m[6] + " " + m[10] + " " + m[14] + "; "
                + m[3] + " " + m[7] + " " + m[11] + " " + m[15] + "]";
    }

    public double trace() {
        return m[0] + m[5] + m[10] + m[15];
    }

    public void setZero() {
        for (int i = 0; i < 16; i++) {
            m[i] = 0;
        }
    }

    public void set(double v11, double v12, double v13, double v14,
                    double v21, double v22, double v23, double v24,
                    double v31, double v32, double v33, double v34,
                    double v41, double v42, double v43, double v44) {
        m[0] = v11;
        m[1] = v21;
        m[2] = v31;
        m[3] = v41;

        m[4] = v12;
        m[5] = v22;
        m[6] = v32;
        m[7] = v42;

        m[8] = v13;
        m[9] = v23;
        m[10] = v33;
        m[11] = v43;

        m[12] = v14;
        m[13] = v24;
        m[14] = v34;
        m[15] = v44;
    }

    public void set(Matrix4x4 A) {
        for (int i = 0; i < 16; i++) {
            this.m[i] = A.m[i];
        }
    }

    public void setDiagonal(Vec3 d) {
        set(d.v[0], 0, 0, 0,
            0, d.v[1], 0, 0,
            0, 0, d.v[2], 0,
            0, 0, 0, 1);
    }

    public void setBasis(Vec3 a, Vec3 b, Vec3 c) {
        setIdentity();
        m[0] = a.v[0];
        m[1] = a.v[1];
        m[2] = a.v[2];
        m[4] = b.v[0];
        m[5] = b.v[1];
        m[6] = b.v[2];
        m[8] = c.v[0];
        m[9] = c.v[1];
        m[10] = c.v[2];
    }

    public void setIdentity() {
        setZero();
        m[0] = 1;
        m[5] = 1;
        m[10] = 1;
        m[15] = 1;
    }

    public void setTranslation(Vec3 t) {
        setIdentity();
        m[12] = t.v[0];
        m[13] = t.v[1];
        m[14] = t.v[2];
    }

    public void setTranslation(double tx, double ty, double tz) {
        set(1, 0, 0, tx,
            0, 1, 0, ty,
            0, 0, 1, tz,
            0, 0, 0, 1);
    }

    public void setScaling(Vec3 s) {
        set(s.v[0], 0, 0, 0,
            0, s.v[1], 0, 0,
            0, 0, s.v[2], 0,
            0, 0, 0, 1);
    }

    public void setScalingUniform(double a) {
        set(a, 0, 0, 0,
            0, a, 0, 0,
            0, 0, a, 0,
            0, 0, 0, 1);
    }

    public void setRotatation(Vec3 axis, double degtheta) {
        double theta = degtheta * Math.PI / 180.0f;
        double x, y, z, s, c, t;
        x = axis.v[0];
        y = axis.v[1];
        z = axis.v[2];
        s = Math.sin(theta);
        c = Math.cos(theta);
        //c = Math.sqrt(1 - s*s);
        t = 1 - c;

        set(t * x * x + c, t * x * y + s * z, t * x * z - s * y, 0,
                t * x * y - s * z, t * y * y + c, t * y * z + s * x, 0,
                t * x * z + s * y, t * y * z - s * x, t * z * z + c, 0,
                0, 0, 0, 1);
    }

    public void setFrustum(double left, double right, double bottom, double top, double znear, double zfar) {
        double temp, temp2, temp3, temp4;
        temp = 2.0 * znear;
        temp2 = right - left;
        temp3 = top - bottom;
        temp4 = zfar - znear;
        m[0] = temp / temp2;
        m[1] = 0;
        m[2] = 0;
        m[3] = 0;
        m[4] = 0;
        m[5] = temp / temp3;
        m[6] = 0;
        m[7] = 0;
        m[8] = (right + left) / temp2;
        m[9] = (top + bottom) / temp3;
        m[10] = (-zfar - znear) / temp4;
        m[11] = -1;
        m[12] = 0;
        m[13] = 0;
        m[14] = (-temp * zfar) / temp4;
        m[15] = 0;
    }

    public void setPerspective(double fovyInDegrees, double aspect, double znear, double zfar) {
        double sine, cotangent, deltaZ;
        double radians = fovyInDegrees / 2 * Math.PI / 180;

        deltaZ = zfar - znear;
        sine = Math.sin(radians);
        
        if ((deltaZ == 0) || (sine == 0) || (aspect == 0)) {
            return;
        }

        cotangent = Math.cos(radians) / sine;

        setIdentity();
        m[0] = cotangent / aspect;
        m[5] = cotangent;
        m[10] = - (zfar + znear) / deltaZ;
        m[11] = -1;
        m[14] = -2 * znear * zfar / deltaZ;
        m[15] = 0;
    }

    public void setOrthographic(double left, double right, double bottom, double top, double znear, double zfar) {
        setIdentity();
        m[0] = 2.0/(right-left);
        m[5] = 2.0/(top-bottom);
        m[10] = -2.0/(zfar-znear);
        m[12] = -(right+left)/(right-left);
        m[13] = -(top+bottom)/(top-bottom);
        m[14] = -(zfar+znear)/(zfar-znear);
    }

    public void setOrthographicStandardAnamorphic(int width, int height, double znear, double zfar) {
        double aspect = (double) width/(double) height;
        setOrthographic( -aspect, aspect, -1.0, 1.0, znear, zfar);
    }

    public void setLookAt(Vec3 eye, Vec3 center, Vec3 up) {
        Vec3 forward = Vec3.sub(center, eye).normalized();
        Vec3 side = Vec3.cross(forward, up).normalized();
        up = Vec3.cross(side, forward);

        setLookatLocalBasis(side, up, forward, eye);
    }

    public void setLookatLocalBasis(Vec3 side, Vec3 up, Vec3 forward, Vec3 eye) {
        m[0] = side.v[0];
        m[4] = side.v[1];
        m[8] = side.v[2];
        m[9] = 0;

        m[1] = up.v[0];
        m[5] = up.v[1];
        m[9] = up.v[2];
        m[10] = 0;

        m[2] = -forward.v[0];
        m[6] = -forward.v[1];
        m[10] = -forward.v[2];
        m[11] = 0;

        m[12] = -(side.v[0]*eye.v[0] + side.v[1]*eye.v[1] + side.v[2]*eye.v[2]);
        m[13] = -(up.v[0]*eye.v[0] + up.v[1]*eye.v[1] + up.v[2]*eye.v[2]);
        m[14] = forward.v[0]*eye.v[0] + forward.v[1]*eye.v[1] + forward.v[2]*eye.v[2];
        m[15] = 1;
    }

    public static Vec3 projectNDC(Vec3 obj, Matrix4x4 modelview, Matrix4x4 projection, int width, int height) {
        double[] in = {obj.v[0], obj.v[1], obj.v[2], 1.0};
        in = modelview.mul(in);
        in = projection.mul(in);

        in[0] /= in[3];
        in[1] /= in[3];
        in[2] /= in[3];

        in[1] *= (double)width/(double)height;
        return new Vec3(in[0], in[1], in[2]);
    }

    public Matrix4x4 add(Matrix4x4 B) {
        Matrix4x4 C = new Matrix4x4();
        for (int i = 0; i < 16; i++) {
            C.m[i] = m[i] + B.m[i];
        }
        return C;
    }

    public Matrix4x4 sub(Matrix4x4 B) {
        Matrix4x4 C = new Matrix4x4();
        for (int i = 0; i < 16; i++) {
            C.m[i] = m[i] - B.m[i];
        }
        return C;
    }

    public Matrix4x4 mul(double a) {
        Matrix4x4 C = new Matrix4x4();
        for (int i = 0; i < 16; i++) {
            C.m[i] = a * m[i];
        }
        return C;
    }

    public void mulInplace(double a) {
        for (int i = 0; i < 16; i++) {
            m[i] *= a;
        }
    }

    public void mulInplace(Matrix4x4 A) {
        set(mul(A, this));
    }

    public static Matrix4x4 mul(Matrix4x4 A, Matrix4x4 B) {
        Matrix4x4 C = new Matrix4x4();

        C.m[0] = A.m[0]*B.m[0] + A.m[4]*B.m[1] + A.m[8]*B.m[2] + A.m[12]*B.m[3];
        C.m[1] = A.m[1]*B.m[0] + A.m[5]*B.m[1] + A.m[9]*B.m[2] + A.m[13]*B.m[3];
        C.m[2] = A.m[2]*B.m[0] + A.m[6]*B.m[1] + A.m[10]*B.m[2] + A.m[14]*B.m[3];
        C.m[3] = A.m[3]*B.m[0] + A.m[7]*B.m[1] + A.m[11]*B.m[2] + A.m[15]*B.m[3];

        C.m[4] = A.m[0]*B.m[4] + A.m[4]*B.m[5] + A.m[8]*B.m[6] + A.m[12]*B.m[7];
        C.m[5] = A.m[1]*B.m[4] + A.m[5]*B.m[5] + A.m[9]*B.m[6] + A.m[13]*B.m[7];
        C.m[6] = A.m[2]*B.m[4] + A.m[6]*B.m[5] + A.m[10]*B.m[6] + A.m[14]*B.m[7];
        C.m[7] = A.m[3]*B.m[4] + A.m[7]*B.m[5] + A.m[11]*B.m[6] + A.m[15]*B.m[7];

        C.m[8] = A.m[0]*B.m[8] + A.m[4]*B.m[9] + A.m[8]*B.m[10] + A.m[12]*B.m[11];
        C.m[9] = A.m[1]*B.m[8] + A.m[5]*B.m[9] + A.m[9]*B.m[10] + A.m[13]*B.m[11];
        C.m[10] = A.m[2]*B.m[8] + A.m[6]*B.m[9] + A.m[10]*B.m[10] + A.m[14]*B.m[11];
        C.m[11] = A.m[3]*B.m[8] + A.m[7]*B.m[9] + A.m[11]*B.m[10] + A.m[15]*B.m[11];

        C.m[12] = A.m[0]*B.m[12] + A.m[4]*B.m[13] + A.m[8]*B.m[14] + A.m[12]*B.m[15];
        C.m[13] = A.m[1]*B.m[12] + A.m[5]*B.m[13] + A.m[9]*B.m[14] + A.m[13]*B.m[15];
        C.m[14] = A.m[2]*B.m[12] + A.m[6]*B.m[13] + A.m[10]*B.m[14] + A.m[14]*B.m[15];
        C.m[15] = A.m[3]*B.m[12] + A.m[7]*B.m[13] + A.m[11]*B.m[15] + A.m[15]*B.m[15];

        return C;
    }

    public static Matrix4x4 mul(Matrix4x4 A1, Matrix4x4 A2, Matrix4x4 A3) {
        return mul(mul(A1, A2), A3);
    }

    public static Matrix4x4 mul(Matrix4x4 A1, Matrix4x4 A2, Matrix4x4 A3, Matrix4x4 A4) {
        return mul(mul(A1, A2, A3), A4);
    }

    public double[] mul(double[] in) {
        double[] out = new double[4];
        for(int i = 0; i < 4; i++) {
        out[i] =
            in[0] * m[0*4+i] +
            in[1] * m[1*4+i] +
            in[2] * m[2*4+i] +
            in[3] * m[3*4+i];
        }
        return out;
    }

    public Vec3 mul(Vec3 vec) {
        Vec3 out = new Vec3();

        out.v[0] = m[0] * vec.v[0] + m[4] * vec.v[1] + m[8] * vec.v[2] + m[12];
        out.v[1] = m[1] * vec.v[0] + m[5] * vec.v[1] + m[9] * vec.v[2] + m[13];
        out.v[2] = m[2] * vec.v[0] + m[6] * vec.v[1] + m[10] * vec.v[2] + m[14];
        double wInv = 1.0 / (m[3] * vec.v[0] + m[7] * vec.v[1] + m[11] * vec.v[2] + m[15]);

        out.scaleInplace(wInv);

        return out;
    }

    public Matrix4x4 transpose() {
        return new Matrix4x4(
                m[0], m[1], m[2], m[3],
                m[4], m[5], m[6], m[7],
                m[8], m[9], m[10], m[11],
                m[12], m[13], m[14], m[15]);
    }

    public Matrix4x4 inverse() {
        Matrix4x4 inv = new Matrix4x4();

        inv.m[0] =   m[5]*m[10]*m[15] - m[5]*m[11]*m[14] - m[9]*m[6]*m[15]
                 + m[9]*m[7]*m[14] + m[13]*m[6]*m[11] - m[13]*m[7]*m[10];
        inv.m[4] =  -m[4]*m[10]*m[15] + m[4]*m[11]*m[14] + m[8]*m[6]*m[15]
                 - m[8]*m[7]*m[14] - m[12]*m[6]*m[11] + m[12]*m[7]*m[10];
        inv.m[8] =   m[4]*m[9]*m[15] - m[4]*m[11]*m[13] - m[8]*m[5]*m[15]
                 + m[8]*m[7]*m[13] + m[12]*m[5]*m[11] - m[12]*m[7]*m[9];
        inv.m[12] = -m[4]*m[9]*m[14] + m[4]*m[10]*m[13] + m[8]*m[5]*m[14]
                 - m[8]*m[6]*m[13] - m[12]*m[5]*m[10] + m[12]*m[6]*m[9];
        inv.m[1] =  -m[1]*m[10]*m[15] + m[1]*m[11]*m[14] + m[9]*m[2]*m[15]
                 - m[9]*m[3]*m[14] - m[13]*m[2]*m[11] + m[13]*m[3]*m[10];
        inv.m[5] =   m[0]*m[10]*m[15] - m[0]*m[11]*m[14] - m[8]*m[2]*m[15]
                 + m[8]*m[3]*m[14] + m[12]*m[2]*m[11] - m[12]*m[3]*m[10];
        inv.m[9] =  -m[0]*m[9]*m[15] + m[0]*m[11]*m[13] + m[8]*m[1]*m[15]
                 - m[8]*m[3]*m[13] - m[12]*m[1]*m[11] + m[12]*m[3]*m[9];
        inv.m[13] =  m[0]*m[9]*m[14] - m[0]*m[10]*m[13] - m[8]*m[1]*m[14]
                 + m[8]*m[2]*m[13] + m[12]*m[1]*m[10] - m[12]*m[2]*m[9];
        inv.m[2] =   m[1]*m[6]*m[15] - m[1]*m[7]*m[14] - m[5]*m[2]*m[15]
                 + m[5]*m[3]*m[14] + m[13]*m[2]*m[7] - m[13]*m[3]*m[6];
        inv.m[6] =  -m[0]*m[6]*m[15] + m[0]*m[7]*m[14] + m[4]*m[2]*m[15]
                 - m[4]*m[3]*m[14] - m[12]*m[2]*m[7] + m[12]*m[3]*m[6];
        inv.m[10] =  m[0]*m[5]*m[15] - m[0]*m[7]*m[13] - m[4]*m[1]*m[15]
                 + m[4]*m[3]*m[13] + m[12]*m[1]*m[7] - m[12]*m[3]*m[5];
        inv.m[14] = -m[0]*m[5]*m[14] + m[0]*m[6]*m[13] + m[4]*m[1]*m[14]
                 - m[4]*m[2]*m[13] - m[12]*m[1]*m[6] + m[12]*m[2]*m[5];
        inv.m[3] =  -m[1]*m[6]*m[11] + m[1]*m[7]*m[10] + m[5]*m[2]*m[11]
                 - m[5]*m[3]*m[10] - m[9]*m[2]*m[7] + m[9]*m[3]*m[6];
        inv.m[7] =   m[0]*m[6]*m[11] - m[0]*m[7]*m[10] - m[4]*m[2]*m[11]
                 + m[4]*m[3]*m[10] + m[8]*m[2]*m[7] - m[8]*m[3]*m[6];
        inv.m[11] = -m[0]*m[5]*m[11] + m[0]*m[7]*m[9] + m[4]*m[1]*m[11]
                 - m[4]*m[3]*m[9] - m[8]*m[1]*m[7] + m[8]*m[3]*m[5];
        inv.m[15] =  m[0]*m[5]*m[10] - m[0]*m[6]*m[9] - m[4]*m[1]*m[10]
                 + m[4]*m[2]*m[9] + m[8]*m[1]*m[6] - m[8]*m[2]*m[5];

        double det = m[0]*inv.m[0] + m[1]*inv.m[4] + m[2]*inv.m[8] + m[3]*inv.m[12];

        if(det == 0) {
            inv.setZero();
            return inv;
        }

        det = 1.0 / det;

        for(int i = 0; i < 16; i++) {
            inv.m[i] = inv.m[i] * det;
        }

        return inv;
    }

}
